#ifndef _POINT_H_
#define _POINT_H_
#include <assert.h>
#include <math.h>
#include <iostream>

#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME

namespace GMapping {

	template <class T>
	struct AxPoint{
		inline AxPoint():x(0),y(0) {}
		inline AxPoint(T _x, T _y):x(_x),y(_y){}
		T x, y;
	};

	template <class T>
	inline AxPoint<T> operator+(const AxPoint<T>& p1, const AxPoint<T>& p2)
	{
		return AxPoint<T>(p1.x+p2.x, p1.y+p2.y);
	}

	template <class T>
	inline AxPoint<T> operator - (const AxPoint<T> & p1, const AxPoint<T> & p2)
	{
		return AxPoint<T>(p1.x-p2.x, p1.y-p2.y);
	}

	template <class T>
	inline AxPoint<T> operator * (const AxPoint<T>& p, const T& v)
	{
		return AxPoint<T>(p.x*v, p.y*v);
	}

	template <class T>
	inline AxPoint<T> operator * (const T& v, const AxPoint<T>& p)
	{
		return AxPoint<T>(p.x*v, p.y*v);
	}

	template <class T>
	inline T operator * (const AxPoint<T>& p1, const AxPoint<T>& p2)
	{
		return p1.x*p2.x+p1.y*p2.y;
	}


	template <class T, class A>
	struct orientedpoint: public AxPoint<T>
	{
		inline orientedpoint() : AxPoint<T>(0,0), theta(0) {};
		inline orientedpoint(const AxPoint<T>& p);
		inline orientedpoint(T x, T y, A _theta): AxPoint<T>(x,y), theta(_theta){}
		inline void normalize();
		inline orientedpoint<T,A> rotate(A alpha)
		{
			T s=sin(alpha), c=cos(alpha);
			A a=alpha+theta;
			a=atan2(sin(a),cos(a));
			return orientedAxPoint(c*this->x-s*this->y,s*this->x+c*this->y,a);
		}
		A theta;
	};


	template <class T, class A>
	void orientedpoint<T,A>::normalize() 
	{
		if (theta >= -M_PI && theta < M_PI)
			return;

		int multiplier = (int)(theta / (2*M_PI));
		theta = theta - multiplier*2*M_PI;
		if (theta >= M_PI)
			theta -= 2*M_PI;
		if (theta < -M_PI)
			theta += 2*M_PI;
	}


	template <class T, class A>
	orientedpoint<T,A>::orientedpoint(const AxPoint<T>& p)
	{
		this->x=p.x;
		this->y=p.y;
		this->theta=0.;
	}


	template <class T, class A>
	orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
		return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
	}

	template <class T, class A>
	orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
		return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
	}

	template <class T, class A>
	orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
		return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
	}

	template <class T, class A>
	orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
		return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
	}

	template <class T, class A>
	orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
	{
		orientedpoint<T,A> delta=p1-p2;
		delta.theta=atan2(sin(delta.theta), cos(delta.theta));
		double s=sin(p2.theta), c=cos(p2.theta);
		return orientedpoint<T,A>(c*delta.x+s*delta.y, 
			-s*delta.x+c*delta.y, delta.theta);
	}

	template <class T, class A>
	orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
	{
		double s=sin(p1.theta), c=cos(p1.theta);
		return orientedpoint<T,A>(c*p2.x-s*p2.y,
			s*p2.x+c*p2.y, p2.theta) + p1;
	}

	template <class T, class A>
	AxPoint<T> absoluteSum(const orientedpoint<T,A>& p1,const AxPoint<T>& p2)
	{
		double s=sin(p1.theta), c=cos(p1.theta);
		return AxPoint<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (AxPoint<T>) p1;
	}

	template <class T>
	struct pointcomparator
	{
		bool operator ()(const AxPoint<T>& a, const AxPoint<T>& b) const 
		{
			return a.x<b.x || (a.x==b.x && a.y<b.y);
		}	
	};

	template <class T>
	struct pointradialcomparator
	{
		AxPoint<T> origin;
		bool operator ()(const AxPoint<T>& a, const AxPoint<T>& b) const 
		{
			AxPoint<T> delta1=a-origin;
			AxPoint<T> delta2=b-origin;
			return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
		}	
	};

	template <class T>
	inline AxPoint<T> pmax(const AxPoint<T>& p1, const AxPoint<T>& p2)
	{
		AxPoint<T> p=p1;
		p.x=p.x>p2.x?p.x:p2.x;
		p.y=p.y>p2.y?p.y:p2.y;
		return p;
	}

	template <class T>
	inline AxPoint<T> pmin(const AxPoint<T>& p1, const AxPoint<T>& p2)
	{
		AxPoint<T> p=p1;
		p.x=p.x<p2.x?p.x:p2.x;
		p.y=p.y<p2.y?p.y:p2.y;
		return p;
	}

	template <class T, class F>
	inline AxPoint<T> interpolate(const AxPoint<T>& p1,  const F& t1, const AxPoint<T>& p2, const F& t2, const F& t3)
	{
		F gain=(t3-t1)/(t2-t1);
		AxPoint<T> p=p1+(p2-p1)*gain;
		return p;
	}

	template <class T, class A, class F>
	inline orientedpoint<T,A> 
		interpolate(const orientedpoint<T,A>& p1,  const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3)
	{
			F gain=(t3-t1)/(t2-t1);
			orientedpoint<T,A> p;
			p.x=p1.x+(p2.x-p1.x)*gain;
			p.y=p1.y+(p2.y-p1.y)*gain;
			double  s=sin(p1.theta)+sin(p2.theta)*gain,
				c=cos(p1.theta)+cos(p2.theta)*gain;
			p.theta=atan2(s,c);
			return p;
	}


	template <class T>
	inline double euclidianDist(const AxPoint<T>& p1, const AxPoint<T>& p2){
		return hypot(p1.x-p2.x, p1.y-p2.y);
	}
	template <class T, class A>
	inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
		return hypot(p1.x-p2.x, p1.y-p2.y);
	}
	template <class T, class A>
	inline double euclidianDist(const orientedpoint<T,A>& p1, const AxPoint<T>& p2){
		return hypot(p1.x-p2.x, p1.y-p2.y);
	}
	template <class T, class A>
	inline double euclidianDist(const AxPoint<T>& p1, const orientedpoint<T,A>& p2 ){
		return hypot(p1.x-p2.x, p1.y-p2.y);
	}



	typedef AxPoint<int> IntPoint;
	typedef AxPoint<double> Point;
	typedef orientedpoint<double, double> OrientedPoint;

}; //end namespace

#endif
